//
// Created by f on 2022/3/25.
//
#include "grid_map.h"

GridMap::GridMap(const float &center_x, const float &center_y, const float &center_yaw,
        const float &grid_dim, const int &half_x_size, const int &half_y_size, const int& layer_id):
        center_x_(center_x), center_y_(center_y), center_yaw_(center_yaw),
        grid_dim_(grid_dim), half_x_size_(half_x_size), half_y_size_(half_y_size),
        layer_id_(layer_id), MapBase()
{
    grids_ = cv::Mat::zeros(2*half_x_size_, 2*half_y_size_, CV_8UC1);
}

GridMap::GridMap(const int& layer_id):layer_id_(layer_id), MapBase(){}

bool GridMap::AddPoint(const double& x, const double& y){
    //转换到网格坐标系下
    double x_trans = x - center_x_;
    double y_trans = y - center_y_;
    double rot_angle = -center_yaw_;
    double x_rot = cos(rot_angle) * x_trans - sin(rot_angle) * y_trans;
    double y_rot = sin(rot_angle) * x_trans + cos(rot_angle) * y_trans;
    //找到对应的网格索引
    int row = floor( x_rot/grid_dim_ + half_x_size_ );
    int col = floor( y_rot/grid_dim_ + half_y_size_ );
    //修改值
    if(col<0 || row<0 || col>=2*half_x_size_ || row>=2*half_y_size_){
        // printf("Point out of grid map\n");
        return false;
    }
    grids_.at<uchar>(2*half_x_size_-row-1, col) = 255;

    return true;
}

uint8_t GridMap::GetValue(const double& x, const double& y){
    //转换到网格坐标系
    double x_trans = x - center_x_;
    double y_trans = y - center_y_;
    double rot_angle = -center_yaw_;
    double x_rot = cos(rot_angle) * x_trans - sin(rot_angle) * y_trans;
    double y_rot = sin(rot_angle) * x_trans + cos(rot_angle) * y_trans;
    //找到对应的网格索引
    int row = floor( x_rot/grid_dim_ + half_x_size_ );
    int col = floor( y_rot/grid_dim_ + half_y_size_ );
    //返回value引用
    if(col<0 || row<0 || col>=2*half_x_size_ || row>=2*half_y_size_){
        std::cerr<<"Point out of grid map\n"<<std::endl;
    }
    return ( grids_.at<uchar>(2*half_x_size_-row-1, col) );
}

bool GridMap::SaveMap(const std::string& save_path,  const std::string& save_type){
    std::string map_img_path = save_path + "gridmap_"
                               + std::to_string(center_x_) + "_"
                               + std::to_string(center_y_) + "_"
                               + std::to_string(center_yaw_) + "_"
                               + std::to_string(grid_dim_);

    if(save_type == "PNG"){
        map_img_path += ".png";
        printf("Saving map file ... ...\n");
        cv::imwrite(map_img_path , grids_);
        printf("Map saved at %s\n", map_img_path.c_str());
        return true;
    }
    else if(save_type == "PGM"){
        map_img_path += ".pgm";
        printf("Saving map file ... ...\n");
        std::vector<int> compression_params;
        compression_params.emplace_back(CV_IMWRITE_PXM_BINARY); //Set to PXM compression
        compression_params.emplace_back(0); //Set type of PXM in our case PGM

        cv::imwrite(map_img_path, grids_, compression_params);
        printf("Map saved at %s\n", map_img_path.c_str());
        return true;
    }
    else{
        printf("Unsupported save type. Supported type: PNG, PGM\n");
        return false;
    }
}

bool GridMap::LoadMap(const std::string& load_path,  const std::string& load_type){
    if(load_type=="PNG" || load_type=="PGM"){
        //截取文件名
        int p = -1;
        for(int i=load_path.size()-1;i>=0;i--) if(load_path[i]=='/') p=i+1;
        p = p==-1 ? 0:p;
        std::string file_name = load_path.substr(p);

        char type_name[3];
        sscanf(file_name.c_str(), "gridmap_%lf_%lf_%lf_%lf.%s", &center_x_, &center_y_, &center_yaw_, &grid_dim_, type_name);

        grids_ = cv::imread(load_path, cv::IMREAD_UNCHANGED);

        half_x_size_ = grids_.rows / 2;
        half_y_size_ = grids_.cols / 2;

        return true;
    }
    else{
        printf("Unsupported map type. Supported type: PNG, PGM\n");
        return false;
    }
}

void GridMap::Clear(){
    grids_ = cv::Mat::zeros(2*half_x_size_, 2*half_y_size_, CV_8UC1);
}

bool GridMap::toCVMat(cv::Mat &m){
    m = grids_.clone();
    return true;
}

double GridMap::Center_x(){
    return center_x_;
}
double GridMap::Center_y(){
    return center_y_;
}
double GridMap::Center_yaw(){
    return center_yaw_;
}
double GridMap::Dim_x(){
    return 2*half_x_size_*grid_dim_;
}
double GridMap::Dim_y(){
    return 2*half_y_size_*grid_dim_;
}
int GridMap::LayerID(){
    return layer_id_;
}